{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/pick.html).  I recommend having both windows open, side-by-side!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from IPython.display import clear_output, display\n",
    "from pydrake.all import (\n",
    "    AbstractValue,\n",
    "    AddMultibodyPlantSceneGraph,\n",
    "    DiagramBuilder,\n",
    "    JacobianWrtVariable,\n",
    "    JointSliders,\n",
    "    LeafSystem,\n",
    "    MeshcatVisualizer,\n",
    "    Parser,\n",
    "    RigidTransform,\n",
    "    RollPitchYaw,\n",
    "    StartMeshcat,\n",
    ")\n",
    "\n",
    "from manipulation import ConfigureParser, running_as_notebook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "wSa5QcU0Piak"
   },
   "source": [
    "# Kinematic Jacobians for pick and place\n",
    "\n",
    "Let's set up the same iiwa + wsg example, with sliders (but without the frames), that we used above.  But this time I'll display the value of the Jacobian $J^G(q)$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "5SjOClhTltPk"
   },
   "outputs": [],
   "source": [
    "class PrintJacobian(LeafSystem):\n",
    "    def __init__(self, plant, frame):\n",
    "        LeafSystem.__init__(self)\n",
    "        self._plant = plant\n",
    "        self._plant_context = plant.CreateDefaultContext()\n",
    "        self._frame = frame\n",
    "        self.DeclareVectorInputPort(\"state\", plant.num_multibody_states())\n",
    "        self.DeclareForcedPublishEvent(self.Publish)\n",
    "\n",
    "    def Publish(self, context):\n",
    "        state = self.get_input_port().Eval(context)\n",
    "        self._plant.SetPositionsAndVelocities(self._plant_context, state)\n",
    "        W = self._plant.world_frame()\n",
    "        J_G = self._plant.CalcJacobianSpatialVelocity(\n",
    "            self._plant_context,\n",
    "            JacobianWrtVariable.kQDot,\n",
    "            self._frame,\n",
    "            [0, 0, 0],\n",
    "            W,\n",
    "            W,\n",
    "        )  ## This is the important line\n",
    "\n",
    "        print(\"J_G:\")\n",
    "        print(\n",
    "            np.array2string(\n",
    "                J_G, formatter={\"float\": lambda x: \"{:5.1f}\".format(x)}\n",
    "            )\n",
    "        )\n",
    "        print(\n",
    "            f\"smallest singular value(J_G): {np.min(np.linalg.svd(J_G, compute_uv=False))}\"\n",
    "        )\n",
    "        clear_output(wait=True)\n",
    "\n",
    "\n",
    "def pick_and_place_jacobians_example():\n",
    "    builder = DiagramBuilder()\n",
    "\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0)\n",
    "    parser = Parser(plant)\n",
    "    ConfigureParser(parser)\n",
    "    parser.AddModelsFromUrl(\"package://manipulation/iiwa_and_wsg.dmd.yaml\")\n",
    "    plant.Finalize()\n",
    "\n",
    "    meshcat.Delete()\n",
    "    visualizer = MeshcatVisualizer.AddToBuilder(\n",
    "        builder, scene_graph.get_query_output_port(), meshcat\n",
    "    )\n",
    "\n",
    "    G = plant.GetBodyByName(\"body\").body_frame()\n",
    "    print_jacobian = builder.AddSystem(PrintJacobian(plant, G))\n",
    "    builder.Connect(\n",
    "        plant.get_state_output_port(), print_jacobian.get_input_port()\n",
    "    )\n",
    "\n",
    "    # If you want to set the initial positions manually, use this:\n",
    "    # plant.SetPositions(plant.GetMyContextFromRoot(context),\n",
    "    #                   plant.GetModelInstanceByName(\"iiwa\"),\n",
    "    #                   [0, 0, 0, 0, 0, 0, 0])\n",
    "\n",
    "    default_interactive_timeout = None if running_as_notebook else 1.0\n",
    "    sliders = builder.AddSystem(JointSliders(meshcat, plant))\n",
    "    diagram = builder.Build()\n",
    "    sliders.Run(diagram, default_interactive_timeout)\n",
    "    meshcat.DeleteAddedControls()\n",
    "\n",
    "\n",
    "meshcat.DeleteAddedControls()\n",
    "pick_and_place_jacobians_example()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "name": "Robotic Manipulation - Basic Pick and Place.ipynb",
   "provenance": []
  },
  "kernelspec": {
   "display_name": "Python 3.10.6 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.6"
  },
  "vscode": {
   "interpreter": {
    "hash": "b0fa6594d8f4cbf19f97940f81e996739fb7646882a419484c72d19e05852a7e"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
